#include "bmi088.h"

bmi088_struct bmi088_data;

void bmi088_init(bmi088_struct* bmi088, SPI_HandleTypeDef* hspi_ptr, GPIO_TypeDef* GPIO_acc_cs, uint16_t GPIO_PIN_acc_cs, GPIO_TypeDef* GPIO_gyro_cs, uint16_t GPIO_PIN_gyro_cs)
{
	bmi088->hspi_ptr = hspi_ptr;
	bmi088->GPIO_acc_cs = GPIO_acc_cs;
	bmi088->GPIO_PIN_acc_cs = GPIO_PIN_acc_cs;
	bmi088->GPIO_gyro_cs = GPIO_gyro_cs;
	bmi088->GPIO_PIN_gyro_cs = GPIO_PIN_gyro_cs;
	
	//accelermeter config
	//switch accelermeter into SPI mode
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_RESET);
	HAL_Delay(10);
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_SET);
	HAL_GPIO_WritePin(bmi088->GPIO_gyro_cs, bmi088->GPIO_PIN_gyro_cs, GPIO_PIN_SET);
	HAL_Delay(10);
	
	uint8_t acc_softreset[2] = {BMI088_ACC_SOFTRESET, 0xB6};
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_RESET);
	HAL_SPI_Transmit(bmi088->hspi_ptr, acc_softreset, 2, 50);
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_SET);
	HAL_Delay(450);
	
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_RESET);
	HAL_Delay(10);
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_SET);
	HAL_GPIO_WritePin(bmi088->GPIO_gyro_cs, bmi088->GPIO_PIN_gyro_cs, GPIO_PIN_SET);
	HAL_Delay(10);
	
	//switch accelermeter into normal mode
	uint8_t acc_normal_tx[2] = {BMI088_ACC_PWR_CTRL, 0x04};
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_RESET);
	HAL_SPI_Transmit(bmi088->hspi_ptr, acc_normal_tx, 2, 50);
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_SET);
	HAL_Delay(450);
	
	//config accelermeter data ready interrupt
	
	uint8_t acc_intio_tx[2] = {BMI088_ACC_INT1_IO_CTRL, 0b00001010};
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_RESET);
	HAL_SPI_Transmit(bmi088->hspi_ptr, acc_intio_tx, 2, 50);
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_SET);
	HAL_Delay(100);
	
	uint8_t acc_int_tx[2] = {BMI088_ACC_INT_MAP_DATA, 0b00000100};
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_RESET);
	HAL_SPI_Transmit(bmi088->hspi_ptr, acc_int_tx, 2, 50);
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_SET);
	HAL_Delay(100);
	
	//gyro config
	uint8_t gyro_softreset[2] = {BMI088_GYRO_SOFTRESET, 0xB6};
	HAL_GPIO_WritePin(bmi088->GPIO_gyro_cs, bmi088->GPIO_PIN_gyro_cs, GPIO_PIN_RESET);
	HAL_SPI_Transmit(bmi088->hspi_ptr, gyro_softreset, 2, 50);
	HAL_GPIO_WritePin(bmi088->GPIO_gyro_cs, bmi088->GPIO_PIN_gyro_cs, GPIO_PIN_SET);
	HAL_Delay(100);
	
	uint8_t gyro_int_ctrl_tx[2] = {BMI088_GYRO_INT_CTRL, 0x80};
	HAL_GPIO_WritePin(bmi088->GPIO_gyro_cs, bmi088->GPIO_PIN_gyro_cs, GPIO_PIN_RESET);
	HAL_SPI_Transmit(bmi088->hspi_ptr, gyro_int_ctrl_tx, 2, 50);
	HAL_GPIO_WritePin(bmi088->GPIO_gyro_cs, bmi088->GPIO_PIN_gyro_cs, GPIO_PIN_SET);
	HAL_Delay(100);
	
	uint8_t gyro_int_io_conf_tx[2] = {BMI088_GYRO_INT3_INT4_IO_CONF, 0X0c};
	HAL_GPIO_WritePin(bmi088->GPIO_gyro_cs, bmi088->GPIO_PIN_gyro_cs, GPIO_PIN_RESET);
	HAL_SPI_Transmit(bmi088->hspi_ptr, gyro_int_io_conf_tx, 2, 50);
	HAL_GPIO_WritePin(bmi088->GPIO_gyro_cs, bmi088->GPIO_PIN_gyro_cs, GPIO_PIN_SET);
	HAL_Delay(100);
														
	uint8_t gyro_int_io_tx[2] = {BMI088_GYRO_INT3_INT4_IO_MAP, 0x01};
	HAL_GPIO_WritePin(bmi088->GPIO_gyro_cs, bmi088->GPIO_PIN_gyro_cs, GPIO_PIN_RESET);
	HAL_SPI_Transmit(bmi088->hspi_ptr, gyro_int_io_tx, 2, 50);
	HAL_GPIO_WritePin(bmi088->GPIO_gyro_cs, bmi088->GPIO_PIN_gyro_cs, GPIO_PIN_SET);
	HAL_Delay(100);
	
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_SET);
	HAL_GPIO_WritePin(bmi088->GPIO_gyro_cs, bmi088->GPIO_PIN_gyro_cs, GPIO_PIN_SET);
}

void bmi088_temp_set(bmi088_struct* bmi088, float temp)
{
	bmi088->temp_target = temp;
}

void bmi088_range_set(bmi088_struct* bmi088, uint8_t acc_range, uint8_t gyro_range)
{
	bmi088->acc_range = acc_range;
	bmi088->gyro_range = gyro_range;
	
	uint8_t acc_range_tx[2] = {BMI088_ACC_RANGE, acc_range};
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_RESET);
	HAL_SPI_Transmit(bmi088->hspi_ptr, acc_range_tx, 2, 50);
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_SET);
	HAL_Delay(10);
	
	uint8_t gyro_range_tx[2] = {BMI088_GYRO_RANGE, gyro_range};
	HAL_GPIO_WritePin(bmi088->GPIO_gyro_cs, bmi088->GPIO_PIN_gyro_cs, GPIO_PIN_RESET);
	HAL_SPI_Transmit(bmi088->hspi_ptr, gyro_range_tx, 2, 50);
	HAL_GPIO_WritePin(bmi088->GPIO_gyro_cs, bmi088->GPIO_PIN_gyro_cs, GPIO_PIN_SET);
	HAL_Delay(10);
}

void bmi088_acc_read(bmi088_struct* bmi088)
{
	uint8_t tx[8] = {	BMI088_ACC_X_LSB | 0x80,
										BMI088_ACC_X_LSB | 0x80,//useless
										BMI088_ACC_X_MSB | 0x80,//useless
										BMI088_ACC_Y_LSB | 0x80,//useless
										BMI088_ACC_Y_MSB | 0x80,//useless
										BMI088_ACC_Z_LSB | 0x80,//useless
										BMI088_ACC_Z_MSB | 0x80,//useless
										0xFF};
	uint8_t rx[8] = {};
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_RESET);
	HAL_SPI_TransmitReceive(bmi088->hspi_ptr, tx, rx, 8, 50);
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_SET);
	bmi088->acc_data.x = rx[2] | (rx[3] << 8);
	bmi088->acc_data.y = rx[4] | (rx[5] << 8);
	bmi088->acc_data.z = rx[6] | (rx[7] << 8);
		
	float factor = pow(2, bmi088->acc_range + 1) * 1.5 * 9.80665 / 32768;
	bmi088->acc.x = bmi088->acc_data.x * factor;
	bmi088->acc.y = bmi088->acc_data.y * factor;
	bmi088->acc.z = bmi088->acc_data.z * factor;
}

void bmi088_gyro_read(bmi088_struct* bmi088)
{
	uint8_t tx[7] = {	BMI088_GYRO_RATE_X_LSB | 0x80,
										BMI088_GYRO_RATE_X_MSB | 0x80,//useless
										BMI088_GYRO_RATE_Y_LSB | 0x80,//useless
										BMI088_GYRO_RATE_Y_MSB | 0x80,//useless
										BMI088_GYRO_RATE_Z_LSB | 0x80,//useless
										BMI088_GYRO_RATE_Z_MSB | 0x80,//useless
										0xFF};
	uint8_t rx[7] = {};
	HAL_GPIO_WritePin(bmi088->GPIO_gyro_cs, bmi088->GPIO_PIN_gyro_cs, GPIO_PIN_RESET);
	HAL_SPI_TransmitReceive(bmi088->hspi_ptr, tx, rx, 7, 50);
	HAL_GPIO_WritePin(bmi088->GPIO_gyro_cs, bmi088->GPIO_PIN_gyro_cs, GPIO_PIN_SET);
	bmi088->gyro_rate_data.x = rx[1] | (rx[2] << 8);
	bmi088->gyro_rate_data.y = rx[3] | (rx[4] << 8);
	bmi088->gyro_rate_data.z = rx[5] | (rx[6] << 8);
	
	float factor = 61.0 / pow(2, bmi088->gyro_range) / 1000.0 * PI / 180.0;
	bmi088->gyro_rate.x = bmi088->gyro_rate_data.x * factor;
	bmi088->gyro_rate.y = bmi088->gyro_rate_data.y * factor;
	bmi088->gyro_rate.z = bmi088->gyro_rate_data.z * factor;
}

void bmi088_temp_ctrl(bmi088_struct* bmi088)
{
	uint8_t tx[4]= {	BMI088_ACC_TEMP_MSB | 0x80,
										0xFF,
										0xFF,
										0xFF};
	uint8_t rx[4] = {};
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_RESET);
	HAL_SPI_TransmitReceive(bmi088->hspi_ptr, tx, rx, 4, 50);
	HAL_GPIO_WritePin(bmi088->GPIO_acc_cs, bmi088->GPIO_PIN_acc_cs, GPIO_PIN_SET);
	int16_t temp_uint11 = ((int16_t)rx[2] << 3) | ((int16_t)rx[3] >> 5);
	int16_t temp_int11 = 0;
	if(temp_int11 > 1023)
	{
		temp_int11 = (int16_t)temp_uint11 - 2048;
	}
	else
	{
		temp_int11 = (int16_t) temp_uint11;
	}
	bmi088->temp = (float)temp_int11 * 0.125 + 23;
	
	if(bmi088->temp < bmi088->temp_target)
	{
		HAL_GPIO_WritePin(GPIOF, GPIO_PIN_6, GPIO_PIN_SET);
	}
	else
	{
		HAL_GPIO_WritePin(GPIOF, GPIO_PIN_6, GPIO_PIN_RESET);
	}
}

void bmi088_txrxCplt_callback()
{
	
}

void bmi088_acc_ready_callback(bmi088_struct* bmi088)
{
	bmi088_acc_read(bmi088);
	bmi088_temp_ctrl(bmi088);
}

void bmi088_gyro_ready_callback(bmi088_struct* bmi088)
{
	bmi088_gyro_read(bmi088);
}